// #include "LidarDriverX.h"
// #include "PointCloudHandler.h"

// #include <chrono>
// #include <thread>

// #pragma region Init
// LidarDriverX::LidarDriverX(LidarConfigurationOptions *options, int id)
//     : _options(options), _lidar_id(id)
// {
//     updateDriverParam(options);

//     _driver.regExceptionCallback(std::bind(&LidarDriverX::exceptionCallback, this, std::placeholders::_1));
//     _driver.regPointCloudCallback(std::bind(&LidarDriverX::pointCloudGetCallback, this), std::bind(&LidarDriverX::pointCloudPutCallback, this, std::placeholders::_1));
// }

// LidarDriverX::~LidarDriverX()
// {
//     stop();
//     _free_cloud_queue.clear();
//     _stuffed_cloud_queue.clear();
//     _pointCloudDataCallback.clear();
// }

// void LidarDriverX::start()
// {
//     _to_exit_process = false;

//     if (_is_pcd_file)
//     {
//         _threadPcdFileProcess.setName("PcdFileProcess");
//         _threadPcdFileProcess.start(&LidarDriverX::threadPcdFileProcess, this);
//     }
//     else
//     {
//         //_rsDriverParam.print();
//         if (!_driver.init(_rsDriverParam))
//         {
//             std::cout << "Driver init failed!" << std::endl;
//             return;
//         }

//         _threadPointCloudProcess.setName("PointCloudProcess");
//         _threadPointCloudProcess.start(&LidarDriverX::threadPointCloudProcess, this);
//         _driver.start();
//     }
// }

// void LidarDriverX::stop()
// {
//     _to_exit_process = true;
//     _driver.stop();
//     _threadPointCloudProcess.tryJoin(5000);
//     _threadPcdFileProcess.tryJoin(5000);
// }

// void LidarDriverX::updateDriverParam(LidarConfigurationOptions *options)
// {
//     // src
//     _rsDriverParam.input_param.msop_port = std::get<uint16_t>(options->GetValue(LidarConfigurationOptions::Data_SrcNet_MsopPort));
//     _rsDriverParam.input_param.difop_port = std::get<uint16_t>(options->GetValue(LidarConfigurationOptions::Data_SrcNet_DifopPort));
//     LidarInputSourceType sourceType = std::get<LidarInputSourceType>(options->GetValue(LidarConfigurationOptions::Data_Source_SrcType));
//     switch (sourceType)
//     {
//     case LidarInputSourceType::ONLINE_LIDAR:
//         _rsDriverParam.input_type = InputType::ONLINE_LIDAR;
//         break;
//     case LidarInputSourceType::PCAP_FILE:
//         _rsDriverParam.input_type = InputType::PCAP_FILE;
//         break;
//     case LidarInputSourceType::RAW_PACKET:
//         _rsDriverParam.input_type = InputType::RAW_PACKET;
//         break;
//     case LidarInputSourceType::PCD_FILE:
//         _is_pcd_file = true;
//         _pcd_file_path = std::get<std::string>(options->GetValue(LidarConfigurationOptions::Data_SrcPcd_FilePath));
//         break;
//     default:
//         break;
//     }
//     LidarManufacturerType manuType = std::get<LidarManufacturerType>(options->GetValue(LidarConfigurationOptions::Data_SrcNet_ManuType));
//     switch (manuType)
//     {
//     case LidarManufacturerType::RS_16:
//         _rsDriverParam.lidar_type = LidarType::RS16;
//         break;
//     case LidarManufacturerType::RS_32:
//         _rsDriverParam.lidar_type = LidarType::RS32;
//         break;
//     case LidarManufacturerType::RS_BP:
//         _rsDriverParam.lidar_type = LidarType::RSBP;
//         break;
//     case LidarManufacturerType::RS_HELIOS:
//         _rsDriverParam.lidar_type = LidarType::RSHELIOS;
//         break;
//     case LidarManufacturerType::RS_128:
//         _rsDriverParam.lidar_type = LidarType::RS128;
//         break;
//     case LidarManufacturerType::RS_80:
//         _rsDriverParam.lidar_type = LidarType::RS80;
//         break;
//     case LidarManufacturerType::RS_M1:
//         _rsDriverParam.lidar_type = LidarType::RSM1;
//         break;
//     default:
//         break;
//     }
//     TransformParam transform = std::get<TransformParam>(options->GetValue(LidarConfigurationOptions::Data_Decode_Transform));
//     _rsDriverParam.decoder_param.transform_param.x = transform.x;
//     _rsDriverParam.decoder_param.transform_param.y = transform.y;
//     _rsDriverParam.decoder_param.transform_param.z = transform.z;
//     _rsDriverParam.decoder_param.transform_param.roll = transform.roll;
//     _rsDriverParam.decoder_param.transform_param.pitch = transform.pitch;
//     _rsDriverParam.decoder_param.transform_param.yaw = transform.yaw;

//     // decode
//     _decodeType = std::get<LidarDecodeType>(options->GetValue(LidarConfigurationOptions::Data_Callback_DecType));
//     if (_decodeType != LidarDecodeType::RAW)
//         _rsDriverParam.decoder_param.dense_points = true;
//     sample_voxel_size = std::get<float>(options->GetValue(LidarConfigurationOptions::Data_Decode_SampleVoxel));

//     // timestamp
//     is_noDelay = std::get<bool>(options->GetValue(LidarConfigurationOptions::Data_Callback_NoDelay));
//     if (is_noDelay)
//         max_buffer_num = 1;
// }
// #pragma endregion

// #pragma region Process
// void LidarDriverX::threadPcdFileProcess(void *pContext)
// {
//     LidarDriverX *pHandler = static_cast<LidarDriverX *>(pContext);
//     pHandler->processPcdFile();
// }

// void LidarDriverX::processPcdFile()
// {
//     pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
//     if (pcl::io::loadPCDFile(_pcd_file_path, *cloud) == -1)
//     {
//         std::cout << "Couldn't read file " << _pcd_file_path << std::endl;
//         return;
//     }
//     TransformParam transform = std::get<TransformParam>(_options->GetValue(LidarConfigurationOptions::Data_Decode_Transform));
//     cloud = PointCloudHandler::GetTransformCloud(cloud, transform);

//     while (!_to_exit_process)
//     {
//         pcl::PointCloud<pcl::PointXYZI>::Ptr _cloud_data(new pcl::PointCloud<pcl::PointXYZI>);
//         pcl::copyPointCloud(*cloud, *_cloud_data);

//         PointCloudData<PointXYZI> data;
//         switch (_decodeType)
//         {
//         case LidarDecodeType::FILTERED:
//             data.cloud_ptr = PointCloudHandler::GetFilteredCloud(_cloud_data, 50, 1.0);
//             break;
//         case LidarDecodeType::DOWNSAMPLED:
//             data.cloud_ptr = PointCloudHandler::GetDownSampledCloud(_cloud_data, sample_voxel_size, sample_voxel_size, sample_voxel_size);
//             break;
//         default:
//             data.cloud_ptr = _cloud_data;
//             break;
//         }
//         auto now = std::chrono::system_clock::now();
//         auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count();
//         data.timestamp = (double)timestamp / 1000000.0;
//         data.lidar_id = _lidar_id;

//         for (PointCloudDataCallback handler : _pointCloudDataCallback)
//         {
//             handler(data);
//         }

//         std::this_thread::sleep_for(std::chrono::milliseconds(100));
//     }
// }

// void LidarDriverX::threadPointCloudProcess(void *pContext)
// {
//     LidarDriverX *pHandler = static_cast<LidarDriverX *>(pContext);
//     pHandler->processPointCloud();
// }

// void LidarDriverX::processPointCloud()
// {
//     while (!_to_exit_process)
//     {
//         // 获取点云数据，释放指针
//         std::shared_ptr<PointCloudMsg> msg = _stuffed_cloud_queue.popWait();
//         if (msg.get() == NULL)
//         {
//             continue;
//         }

//         pcl::PointCloud<PointXYZI>::Ptr pcl_pointcloud(new pcl::PointCloud<PointXYZI>);
//         pcl_pointcloud->points.swap(msg->points);
//         pcl_pointcloud->height = msg->height;
//         pcl_pointcloud->width = msg->width;
//         pcl_pointcloud->is_dense = msg->is_dense;

//         _free_cloud_queue.push(msg);

//         PointCloudData<PointXYZI> data;
//         switch (_decodeType)
//         {
//         case LidarDecodeType::FILTERED:
//             data.cloud_ptr = PointCloudHandler::GetFilteredCloud(pcl_pointcloud, 50, 1.0);
//             break;
//         case LidarDecodeType::DOWNSAMPLED:
//             data.cloud_ptr = PointCloudHandler::GetDownSampledCloud(pcl_pointcloud, sample_voxel_size, sample_voxel_size, sample_voxel_size);
//             break;
//         default:
//             data.cloud_ptr = pcl_pointcloud;
//             break;
//         }
//         data.timestamp = msg->timestamp;
//         data.sequence = msg->seq;
//         data.frame_id = msg->frame_id;
//         data.lidar_id = _lidar_id;

//         for (PointCloudDataCallback handler : _pointCloudDataCallback)
//         {
//             handler(data);
//         }
//     }
// }
// #pragma endregion

// #pragma region Callback
// bool LidarDriverX::addPointCloudMsgCallback(PointCloudDataCallback callback)
// {
//     _pointCloudDataCallback.push_back(callback);
//     return true;
// }

// void LidarDriverX::exceptionCallback(const Error &code)
// {
//     std::cout << "Exception: " << code.toString() << std::endl;
// }

// std::shared_ptr<PointCloudMsg> LidarDriverX::pointCloudGetCallback()
// {
//     std::shared_ptr<PointCloudMsg> msg = _free_cloud_queue.pop();
//     if (msg.get() != NULL)
//     {
//         return msg;
//     }

//     return std::make_shared<PointCloudMsg>();
// }

// void LidarDriverX::pointCloudPutCallback(std::shared_ptr<PointCloudMsg> msg)
// {
//     auto len = _stuffed_cloud_queue.push(msg);
//     if (len > max_buffer_num)
//         _stuffed_cloud_queue.pop();
// }
// #pragma endregion